void init_clock(u32int frequency) {
	u32int divisor = 1193180 / frequency;
	u8int l = (u8int)(divisor & 0xFF);
	u8int h = (u8int)((divisor >> 8) & 0xFF);
    
	outb(0x43, 0x36);
	outb(0x40, l);
	outb(0x40, h);
}

void update_msclock() {
	millisecond_clock++;
	millisecond_clock &= 0x1FFFFFFF; //Wrap at the same range as SmallIntegers
	if(millisecond_clock % 1000 == 0) {
		printNow();
	}
}

void printNow() {
	char hour, min, sec, statusB;
	
	outb(0x70, 4);
	hour = inb(0x71);
	outb(0x70, 2);
	min = inb(0x71);
	outb(0x70, 0);
	sec = inb(0x71);
	outb(0x70, 11);
	statusB = inb(0x71);
	
	// Set output loc & color, preserving originals
	preserve_formatting();
	
	set_fgbg(WHITE, BLUE);
	set_cursor(72, 0);
	printf("        ");
	set_cursor(72, 0);
	
	if((statusB & 4) == 0) {
		//printf("is bcd\n");
		printf("%d:%d:%d\n", (int)bcd(hour), (int)bcd(min), (int)bcd(sec));	
	}
	else {
		//printf("not bcd\n");
		printf("%d:%d:%d\n", (int)hour, (int)min, (int)sec);
	}
	
	restore_formatting();
}

char bcd(char v) {
	return (v / 16) * 10 + (v % 16);
}
